#include "stdafx.h"
#include "Tools.h"
#define square( x ) ( x * x )

namespace Tools
{
	// Calculate distance Euclidean between 2 points p1 and p2
	double distance(const CvPoint &p1, const CvPoint &p2)
	{
		return sqrt((double)(square((p1.x - p2.x)) + square((p1.y - p2.y))));
	}

	// Calculate distance Euclidean between point p0 and line v0v1
	double distance(const CvPoint &p0, const CvPoint &v0, const CvPoint &v1)
	{
		if ((v0.x == v1.x) && (v0.y == v1.y))
		{
			distance(p0,v0);
		}
		else
		{
			// Find value absolute of the cross-product vector (v0v1 * v0p0)
			int cross_product = abs(((v1.x - v0.x) * (p0.y - v0.y)) - ((v1.y - v0.y) * (p0.x - v0.x)));
			return ((double) cross_product) / distance(v0,v1);
		}
	}

	// Calculate estimated stroke width
	// @source: source image
	int calculate_estimated_stroke_width(const IplImage *source)
	{
		int hash_table_size = max((int) source->width, (int) source->height) + 1;

		// A hash table. 
		// runlength_hash_table[i] = number of lines in the image whose length = i
		int* runlength_hash_table = new int [hash_table_size];
		for (int i = 0; i< hash_table_size; i++)
		{
			runlength_hash_table[i] = 0;
		}
	
		// Scan image horizontally, accumulate line whose length = i into runlength_hash_table[i]
		for (int i = 0; i < (int) source->height; i++)
		{
			int runlength = 0;
			for (int j = 0; j < (int) source->width; j++)
			{
				if (((uchar *)(source->imageData + i * source->widthStep))[j] == 0)
				{
					runlength++;
				}
				else
				{
					if (runlength > 0)
					{
						runlength_hash_table[runlength]++;
					}
					runlength = 0;
				}
			}
		}
	

		// Scan image vertically, accumulate line whose length = i into runlength_hash_table[i]
		for (int j = 0; j < (int) source->width; j++)
		{
			int runlength = 0;
			for (int i = 0; i < (int) source->height; i++)
			{
				if (((uchar *)(source->imageData + i * source->widthStep))[j] == 0)
				{
					runlength++;
				}
				else
				{
					if (runlength > 0)
					{
						runlength_hash_table[runlength]++;
					}
					runlength = 0;
				}
			}
		}


		// Number of lines which are most frequent
		int max_frequent = 0;

		// Estimated stroke width is width of the line that is most frequent
		int estimated_stroke_width = 0;
		for (int i = 0; i< hash_table_size; i++)
		{
			if (max_frequent < runlength_hash_table[i])
			{
				max_frequent = runlength_hash_table[i];
				estimated_stroke_width = i;
			}
		}

		delete [] runlength_hash_table;

		return estimated_stroke_width;
	}

	// Find contour of an image
	// @source: source image
	// @contour: output, vector that contains contour of source image
	void find_contour(const IplImage* source, vector<CvPoint> &contour)
	{
		contour.clear();
		for (int y = 0; y < source->height; y++)
		{
			for (int x = 0; x < source->width; x++)
			{
				// Check if current point is a contour point
				if (Tools::check_contour_point(source,x,y))
				{
					contour.push_back(cvPoint(x,y));
				}
			}
		}
		return;
	}

	// Check if the specified point is a contour point
	// @source: source image
	// @x: X cordinate of the point
	// @y: Y cordinate of the point
	bool check_contour_point(const IplImage* source, int x, int y)
	{
		if ((x < 0) || (y < 0) || (x >= source->width) || (y >= source->height))
		{
			return false;
		}

		if (((uchar *)(source->imageData + y * source->widthStep))[x] == 0)
		{
			if ((y - 1 < 0) || (x - 1 < 0) || (y + 1 >= source->height) || (x + 1 >= source->width) )
			{
				return true;
			}
			else
			{
				if	(	(((uchar *)(source->imageData + (y - 1) * source->widthStep))[x] == 255)	|| 
						(((uchar *)(source->imageData + (y + 1) * source->widthStep))[x] == 255)	||
						(((uchar *)(source->imageData + y * source->widthStep))[x - 1] == 255)		||
						(((uchar *)(source->imageData + y * source->widthStep))[x + 1] == 255) 
					)
				{
					return true;
				}
			}

		}
		return false;
	}

	// Remove spurious loops in image
	// @source: input image
	// @dest: destination image
	void remove_spurious_loops(const IplImage *source, IplImage *dest)
	{
		if (dest != source)
		{
			cvCopy(source,dest);
		}
		/*------------------- TEMPLATE 1 ------------------------*/
		/*********************************************************
							  a * b
							  * 0 *
			       			  d * c
		**********************************************************/

		int** mask = new int * [3];
			
		for (int k = -1; k < 2; k++)
		{
			mask[k+1] = new int [3];
		}


		for (int i = 1; i< (int) dest->height - 1; i++)
		{
			for (int j = 1; j < (int) dest->width - 1; j++)
			{
				for (int k = -1; k < 2; k++)
				{
					for (int l = -1; l < 2; l++)
					{
						uchar pixel_value = (((uchar *)(dest->imageData + (i + k) * dest->widthStep))[j + l] == 255) ? 0 : 1;
						mask[k+1][l+1] = 1 - pixel_value;
						if ((k == 0) && (l == 0))
						{
							mask[k+1][l+1] = pixel_value;
						}
					}
				}

				if ((mask[0][1] + mask[1][0] + mask[1][1] + mask[1][2] +  mask[2][1]) == 0)
				{
					if (mask[0][0] == 1)
					{
						((uchar *)(dest->imageData + (i - 1) * dest->widthStep))[j] = 255;
						((uchar *)(dest->imageData + (i) * dest->widthStep))[j - 1] = 255;
					}
					else if (mask[0][2] == 1)
					{
						((uchar *)(dest->imageData + (i - 1) * dest->widthStep))[j] = 255;
						((uchar *)(dest->imageData + (i) * dest->widthStep))[j + 1] = 255;

					}
					else if (mask[2][0] == 1)
					{
						((uchar *)(dest->imageData + (i) * dest->widthStep))[j - 1] = 255;
						((uchar *)(dest->imageData + (i + 1) * dest->widthStep))[j] = 255;
					}
					else if (mask[2][2] == 1)
					{
						((uchar *)(dest->imageData + (i + 1) * dest->widthStep))[j] = 255;
						((uchar *)(dest->imageData + (i) * dest->widthStep))[j + 1] = 255;
					}
				}
			}
		}

		// Delete mask
		for (int k = 0; k < 3; k++)
		{
			delete [] mask[k];
		}
		delete [] mask;

		/*------------------- TEMPLATE 2 ------------------------*/
		/*********************************************************
							  a * * b
							  * 0 0 *
			       			  d * * c
		**********************************************************/

		mask = new int * [3];
			
		for (int k = -1; k < 2; k++)
		{
			mask[k+1] = new int [4];
		}
		for (int i = 1; i< (int) dest->height - 1; i++)
		{
			for (int j = 1; j < (int) dest->width - 2; j++)
			{
				for (int k = -1; k < 2; k++)
				{
					for (int l = -1; l < 3; l++)
					{
						uchar pixel_value = (((uchar *)(dest->imageData + (i + k) * dest->widthStep))[j + l] == 255) ? 0 : 1;
						mask[k+1][l+1] = 1 - pixel_value;
						if (((k == 0) && (l == 0)) || ((k == 0) && (l == 1)))
						{
							mask[k+1][l+1] = pixel_value;
						}
					}
				}

				if ((mask[0][1] + mask[1][0] + mask[1][1] + mask[1][2] +  mask[2][1] + mask[0][2] + mask[1][3] + mask[2][2]) == 0)
				{
					if (mask[0][0] == 1)
					{
						((uchar *)(dest->imageData + (i - 1) * dest->widthStep))[j] = 255;
						((uchar *)(dest->imageData + (i) * dest->widthStep))[j - 1] = 255;
					}
					else if (mask[0][3] == 1)
					{
						((uchar *)(dest->imageData + (i - 1) * dest->widthStep))[j+1] = 255;
						((uchar *)(dest->imageData + (i) * dest->widthStep))[j + 2] = 255;

					}
					else if (mask[2][0] == 1)
					{
						((uchar *)(dest->imageData + (i) * dest->widthStep))[j - 1] = 255;
						((uchar *)(dest->imageData + (i + 1) * dest->widthStep))[j] = 255;
					}
					else if (mask[2][3] == 1)
					{
						((uchar *)(dest->imageData + (i + 1) * dest->widthStep))[j + 1] = 255;
						((uchar *)(dest->imageData + (i) * dest->widthStep))[j + 2] = 255;
					}
				}
			}
		}

		// Delete mask
		for (int k = 0; k < 3; k++)
		{
			delete [] mask[k];
		}
		delete [] mask;


		/*------------------- TEMPLATE 3 ------------------------*/
		/*********************************************************
							  a * b
							  * 0 *
							  * 0 *
			       			  d * c
		**********************************************************/
		mask = new int * [4];
			
		for (int k = -1; k < 3; k++)
		{
			mask[k+1] = new int [3];
		}
		for (int i = 1; i< (int) dest->height - 2; i++)
		{
			for (int j = 1; j < (int) dest->width - 1; j++)
			{
				for (int k = -1; k < 3; k++)
				{
					for (int l = -1; l < 2; l++)
					{
						uchar pixel_value = (((uchar *)(dest->imageData + (i + k) * dest->widthStep))[j + l] == 255) ? 0 : 1;
						mask[k+1][l+1] = 1 - pixel_value;
						if (((k == 0) && (l == 0)) || ((k == 1) && (l == 0)))
						{
							mask[k+1][l+1] = pixel_value;
						}
					}
				}

				if ((mask[0][1] + mask[1][0] + mask[1][1] + mask[1][2] +  mask[2][1] + mask[2][0] + mask[3][1] + mask[2][2]) == 0)
				{
					if (mask[0][0] == 1)
					{
						((uchar *)(dest->imageData + (i - 1) * dest->widthStep))[j] = 255;
						((uchar *)(dest->imageData + (i) * dest->widthStep))[j - 1] = 255;
					}
					else if (mask[0][2] == 1)
					{
						((uchar *)(dest->imageData + (i - 1) * dest->widthStep))[j] = 255;
						((uchar *)(dest->imageData + (i) * dest->widthStep))[j + 1] = 255;

					}
					else if (mask[3][0] == 1)
					{
						((uchar *)(dest->imageData + (i + 1) * dest->widthStep))[j - 1] = 255;
						((uchar *)(dest->imageData + (i + 2) * dest->widthStep))[j] = 255;
					}
					else if (mask[3][2] == 1)
					{
						((uchar *)(dest->imageData + (i + 1) * dest->widthStep))[j + 1] = 255;
						((uchar *)(dest->imageData + (i + 2) * dest->widthStep))[j] = 255;
					}
				}
			}
		}
		return;
	}

	// Find approximate shortest distance from a specified point to contour
	// @source: original image
	// @point: specified point
	// Return: approximate shortest distance
	double find_approximate_shortest_distance_to_contour(const IplImage* source, CvPoint point)
	{
		vector<CvPoint> key_points;
		key_points.clear();
		// Find key point P1
		for(int i = point.y; i > 0; i--)
		{
			// Check if current point is a contour point
			if (Tools::check_contour_point(source,point.x,i))
			{
				key_points.push_back(cvPoint(point.x,i));
				break;
			}
		} 
		// Find key point P5
		for(int i = point.y; i < source->height; i++)
		{
			// Check if current point is a contour point
			if (Tools::check_contour_point(source,point.x,i))
			{
				key_points.push_back(cvPoint(point.x,i));
				break;
			}
		} 
		// Find key point P7
		for(int i = point.x; i > 0; i--)
		{
			// Check if current point is a contour point
			if (Tools::check_contour_point(source,i,point.y))
			{
				key_points.push_back(cvPoint(i,point.y));
				break;
			}
		} 
		// Find key point P3
		for(int i = point.x; i < source->width; i++)
		{
			// Check if current point is a contour point
			if (Tools::check_contour_point(source,i,point.y))
			{
				key_points.push_back(cvPoint(i,point.y));
				break;
			}
		} 
		// Find key point P2
		for(int i = 0; ((point.x + i < source->width) && (point.y - i > 0)); i++)
		{
			// Check if one of 8-neighbour pixels is contour point
			bool found1 = false;
			for (int k = -1; k < 2; k++)
			{
				bool found = false;
				for (int l = -1; l < 2; l++)
				{
					if (Tools::check_contour_point(source,point.x + i + l, point.y - i + k))
					{
						found = true;
						key_points.push_back(cvPoint(point.x + i + l, point.y - i + k));
						break;
					}
				}
				if (found)
				{
					found1 = true;
					break;
				}
			}
			if(found1)
			{
				break;
			}
		}

		// Find key point P4
		for(int i = 0; ((point.x + i < source->width) && (point.y + i < source->height)); i++)
		{
			// Check if one of 8-neighbour pixels is contour point
			bool found1 = false;
			for (int k = -1; k < 2; k++)
			{
				bool found = false;
				for (int l = -1; l < 2; l++)
				{
					if (Tools::check_contour_point(source,point.x + i + l, point.y + i + k))
					{
						found = true;
						key_points.push_back(cvPoint(point.x + i + l, point.y + i + k));
						break;
					}
				}
				if (found)
				{
					found1 = true;
					break;
				}
			}
			if (found1)
			{
				break;
			}
		}

		// Find key point P6
		for(int i = 0; ((point.x - i > 0) && (point.y + i < source->height)); i++)
		{
			// Check if one of 8-neighbour pixels is contour point
			bool found1 = false;
			for (int k = -1; k < 2; k++)
			{
				bool found = false;
				for (int l = -1; l < 2; l++)
				{
					if (Tools::check_contour_point(source,point.x - i + l, point.y + i + k))
					{
						found = true;
						key_points.push_back(cvPoint(point.x - i + l, point.y + i + k));
						break;
					}
				}
				if (found)
				{
					found1 = true;
					break;
				}
			}
			if(found1)
			{
				break;
			}
		}

		// Find key point P8
		for(int i = 0; ((point.x - i > 0) && (point.y - i > 0)); i++)
		{
			// Check if one of 8-neighbour pixels is contour point
			bool found1 = false;
			for (int k = -1; k < 2; k++)
			{
				bool found2 = false;
				for (int l = -1; l < 2; l++)
				{
					if (Tools::check_contour_point(source,point.x - i + l, point.y - i + k))
					{
						found2 = true;
						key_points.push_back(cvPoint(point.x - i + l, point.y - i + k));
						break;
					}
				}
				if (found2)
				{
					found1 = true;
					break;
				}
			}
			if (found1)
			{
				break;
			}
		}

		// Find shortest distance
		double min_distance = 100000.0;
		for (int i = 0; i < (int) key_points.size(); i++)
		{
			double distance = Tools::distance(point,key_points[i]);
			if (min_distance > distance)
			{
				min_distance = distance;
			}
		}

		return min_distance;
	}

	// Find connected components in a set of points
	// @S: the set that we want to its connected components
	// Return: list of connected components of S
	void find_connected_components(set<FeaturePoint> S, vector<set<FeaturePoint>> &result)
	{
		while(!S.empty())
		{
			// queue used for BFS algorithm
			list<FeaturePoint> queue;
			set<FeaturePoint> subset;
			// Get the first point of S
			FeaturePoint first_point = (*S.begin());
			// Put to subset
			subset.insert(first_point);
			// Put to queue
			queue.push_back(first_point);
			while(!queue.empty())
			{
				FeaturePoint front_queue = queue.front();
				queue.pop_front();
				// Find neighbour points in S
				for(int k = -1; k < 2; k++)
				{
					for(int l = -1; l < 2; l++)
					{
						FeaturePoint p;
						p.position = cvPoint(front_queue.position.x + l, front_queue.position.y + k);
						// If there is a point in S that is the neighbour of front_queue
						// Put it to subset
						// Then, put it to queue
						// Finally, remove it from S
						if (S.find(p) != S.end())
						{
							subset.insert(p);
							queue.push_back(p);
							S.erase(p);
						}
					}
				}
			}
			result.push_back(subset);
		}
		return;
	}

	// Get all elements in a substroke
	set<CvPoint,ImagePointComp> get_substroke_elements(CvPoint first_point, IplImage* map, CvScalar color, CvScalar &combine_color)
	{
		set<CvPoint,ImagePointComp> retval;
		list<CvPoint> next_points;
		next_points.clear();
		next_points.push_back(first_point);
		bool iscombined = false;
		
		while(!next_points.empty())
		{
			CvPoint current_point = next_points.front();

			// Generate neighbour points
			for (int i = -1; i< 2; i++)
			{
				for (int j = -1; j< 2; j++)
				{
					if( (i + j == -1) || (i + j == 1))
					{
						CvPoint neighbour_point;
						neighbour_point.x = current_point.x + j;
						neighbour_point.y = current_point.y + i;
						if ((neighbour_point.x < 0) || (neighbour_point.x >= map->width) || (neighbour_point.y < 0) || (neighbour_point.y >= map->height))
						{
							continue;
						}
					
						else
						{
							CvScalar pixel_color;
							pixel_color = cvGet2D(map,neighbour_point.y, neighbour_point.x);
							if ((pixel_color.val[0] == color.val[0]) && (pixel_color.val[1] == color.val[1]) && (pixel_color.val[2] == color.val[2]))
							{
								list<CvPoint>::iterator it;
								bool is_contained_in_list = false;
								// Check if generated points are in list or in result set
								// If yes, ignore them
								if (retval.find(neighbour_point) != retval.end())
								{
									continue;
								}
								for (it = next_points.begin(); it != next_points.end(); it++)
								{

									if ((neighbour_point.x == (*it).x) && (neighbour_point.y == (*it).y))
									{
										is_contained_in_list = true;
										break;
									}

								}
								if (is_contained_in_list)
								{
									continue;
								}

								next_points.push_back(neighbour_point);

							}
							else if (((pixel_color.val[0] > 0) && (pixel_color.val[0] < 255)) ||((pixel_color.val[1] > 0) && (pixel_color.val[1] < 255)) || ((pixel_color.val[2] > 0) && (pixel_color.val[2] < 255)))
							{
								combine_color = pixel_color;
							}
						}
					}
				}
			}
			// Put current point in the result set
			// And then, pop it from list
			retval.insert(current_point);
			next_points.pop_front();
		}
		return retval;

	}
}

